{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {
    "colab_type": "text",
    "id": "EgiF12Hf1Dhs"
   },
   "source": [
    "** This notebook has a few of my favorite interactive examples. **\n",
    "\n",
    "# Notebook setup\n",
    "\n",
    "The following cell will:\n",
    "- on Colab (only), install Drake to `/opt/drake`, install Drake's prerequisites via `apt`, and add pydrake to `sys.path`.  This will take approximately two minutes on the first time it runs (to provision the machine), but should only need to reinstall once every 12 hours.  If you navigate between notebooks using Colab's \"File->Open\" menu, then you can avoid provisioning a separate machine for each notebook.\n",
    "- launch a server for our 3D visualizer (MeshCat) that will be used for the remainder of this notebook.\n",
    "\n",
    "You will need to rerun this cell if you restart the kernel, but it should be fast because the machine will already have drake installed."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "colab": {},
    "colab_type": "code",
    "id": "eeMrMI0-1Dhu"
   },
   "outputs": [],
   "source": [
    "import importlib\n",
    "import sys\n",
    "from urllib.request import urlretrieve\n",
    "\n",
    "if 'google.colab' in sys.modules and importlib.util.find_spec('manipulation') is None:\n",
    "    urlretrieve(f\"http://manipulation.csail.mit.edu/scripts/setup/setup_manipulation_colab.py\",\n",
    "                \"setup_manipulation_colab.py\")\n",
    "    from setup_manipulation_colab import setup_manipulation\n",
    "    setup_manipulation(manipulation_sha='master', drake_version='20201116', drake_build='nightly')\n",
    "\n",
    "# Determine if this notebook is currently running as a notebook or a unit test.\n",
    "from IPython import get_ipython\n",
    "running_as_notebook = get_ipython() and hasattr(get_ipython(), 'kernel')\n",
    "\n",
    "# Start a single meshcat server instance to use for the remainder of this notebook.\n",
    "from meshcat.servers.zmqserver import start_zmq_server_as_subprocess\n",
    "server_args = []\n",
    "if 'google.colab' in sys.modules:\n",
    "    server_args = ['--ngrok_http_tunnel']\n",
    "proc, zmq_url, web_url = start_zmq_server_as_subprocess(server_args=server_args)\n",
    "\n",
    "import numpy as np\n",
    "from ipywidgets import Textarea\n",
    "from functools import partial\n",
    "import meshcat\n",
    "\n",
    "import pydrake.all\n",
    "from pydrake.all import RigidTransform, RotationMatrix, RollPitchYaw\n",
    "from manipulation.scenarios import AddIiwa, AddWsg, AddShape, AddTwoLinkIiwa, AddRgbdSensors\n",
    "from manipulation.jupyter_widgets import MakePoseSlidersThatPublishOnCallback, MakeJointSlidersThatPublishOnCallback\n",
    "\n",
    "pydrake.common.set_log_level(\"warn\");"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "colab_type": "text",
    "id": "9iFDN8c7neLH"
   },
   "source": [
    "# Visualizing Jacobian-based control"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "colab": {},
    "colab_type": "code",
    "id": "WKjiU3BgMUa7"
   },
   "outputs": [],
   "source": [
    "import numpy as np\n",
    "from IPython.display import display\n",
    "from ipywidgets import FloatSlider, Textarea, Layout\n",
    "\n",
    "from pydrake.all import (DiagramBuilder, AddMultibodyPlantSceneGraph, RigidTransform, ConnectMeshcatVisualizer,\n",
    "                         JacobianWrtVariable, Parser, FixedOffsetFrame,\n",
    "                         PiecewisePolynomial, MathematicalProgram, LinearConstraint, Solve)\n",
    "from manipulation.utils import FindResource\n",
    "from manipulation.meshcat_utils import plot_mathematical_program\n",
    "\n",
    "\n",
    "# This one is specific to this notebook, but I'm putting it in the header to make it less distracting.\n",
    "def Visualizer(MakeMathematicalProgram):\n",
    "    builder = DiagramBuilder()\n",
    "\n",
    "    plant, scene_graph = AddMultibodyPlantSceneGraph(builder, time_step=1.0)\n",
    "    twolink = AddTwoLinkIiwa(plant, q0=[0.0, 0.0])\n",
    "    hand = plant.GetFrameByName(\"iiwa_link_ee\")\n",
    "    plant.Finalize()\n",
    "\n",
    "    visualizer = ConnectMeshcatVisualizer(builder,\n",
    "                                 scene_graph,\n",
    "                                 zmq_url=zmq_url)\n",
    "    diagram = builder.Build()\n",
    "    context = diagram.CreateDefaultContext()\n",
    "    plant_context = plant.GetMyContextFromRoot(context)\n",
    "\n",
    "    visualizer.vis.delete()\n",
    "    #visualizer.vis[\"/Background\"].set_property('visible',False)\n",
    "    visualizer.vis[\"/Background\"].set_property('top_color', [0, 0, 0])\n",
    "    visualizer.vis[\"/Background\"].set_property('bottom_color', [0, 0, 0])\n",
    "    visualizer.vis[\"/Grid\"].set_property('visible',False)\n",
    "    visualizer.load()\n",
    "\n",
    "    jacobian = Textarea(value=\"\", description=\"J_G: \", layout={'width':'200pm','height':'50px'}, style={'description_width':'initial'})\n",
    "    display(jacobian)\n",
    "\n",
    "    X, Y = np.meshgrid(np.linspace(-5, 5, 35), np.linspace(-5, 5, 31))\n",
    "\n",
    "    def visualize(q, v_Gdesired=[1.0, 0.0], t=None):\n",
    "        if t:\n",
    "            context.SetTime(t)\n",
    "        plant.SetPositions(plant_context, q)\n",
    "        diagram.Publish(context)\n",
    "\n",
    "        J_G = plant.CalcJacobianTranslationalVelocity(plant_context, JacobianWrtVariable.kQDot, hand, [0,0,0], plant.world_frame(), plant.world_frame())\n",
    "        J_G = J_G[[0,2],:]  # Ignore Y.\n",
    "        jacobian.value = np.array2string(J_G, formatter={'float': lambda x: \"{:5.2f}\".format(x)})\n",
    "\n",
    "        prog = MakeMathematicalProgram(q, J_G, v_Gdesired)\n",
    "        result = Solve(prog)\n",
    "        v = visualizer.vis[\"QP\"]\n",
    "        plot_mathematical_program(v, prog, X, Y, result=result)\n",
    "        # TODO: Add set_object to meshcat.Animation\n",
    "        if False: # meshcat._is_recording:\n",
    "            with visualizer._animation.at_frame(\n",
    "                    v, visualizer._recording_frame_num) as m:\n",
    "                plot_mathematical_program(m, prog, X, Y, result=result)\n",
    "\n",
    "    return visualize, visualizer\n",
    "\n",
    "def MakeMathematicalProgram(q, J_G, v_Gdesired):\n",
    "    prog = MathematicalProgram()\n",
    "    v = prog.NewContinuousVariables(2, 'v')\n",
    "    v_max = 3.0 \n",
    "\n",
    "    error = J_G.dot(v) - v_Gdesired\n",
    "    prog.AddCost(error.dot(error))\n",
    "    prog.AddBoundingBoxConstraint(-v_max, v_max, v)\n",
    "\n",
    "    return prog\n",
    "\n",
    "visualize, _ = Visualizer(MakeMathematicalProgram)\n",
    "\n",
    "q = [-np.pi/2.0+0.5, 1.0]\n",
    "v_Gdesired = [0.5, 0.]\n",
    "visualize(q, v_Gdesired)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "colab": {},
    "colab_type": "code",
    "id": "1bbb01FqnxVP"
   },
   "outputs": [],
   "source": [
    "visualize, _ = Visualizer(MakeMathematicalProgram)\n",
    "\n",
    "v_Gdesired = [1.0, 0.0]\n",
    "T = 2.\n",
    "qtraj = PiecewisePolynomial.FirstOrderHold(\n",
    "    [0, T], np.array([[-np.pi / 2.0 + 1., -np.pi / 2.0 - 1.], [2., -2.]]))\n",
    "visualize(qtraj.value(0), v_Gdesired)\n",
    "\n",
    "def _t_callback(change):\n",
    "    visualize(qtraj.value(change.new), v_Gdesired)\n",
    "\n",
    "slider = FloatSlider(value=0, min=0, max=T, step=0.05, continuous_update=True, description=\"t\", layout=Layout(width=\"'100'\"))\n",
    "slider.observe(_t_callback, names='value')\n",
    "display(slider)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Scoring grasp candidates"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "import open3d as o3d\n",
    "from pydrake.all import ( \n",
    "    AddMultibodyPlantSceneGraph, ConnectMeshcatVisualizer, \n",
    "    DiagramBuilder, RigidTransform, RotationMatrix, Box,    \n",
    "    CoulombFriction, FindResourceOrThrow, FixedOffsetFrame, \n",
    "    GeometryInstance, MeshcatContactVisualizer, Parser, PlanarJoint,  \n",
    "    JointIndex, Simulator, ProcessModelDirectives, LoadModelDirectives\n",
    ")\n",
    "from manipulation.meshcat_utils import draw_open3d_point_cloud, draw_points\n",
    "from manipulation.open3d_utils import create_open3d_point_cloud\n",
    "\n",
    "def grasp_candidate_cost(plant_context, cloud, plant, scene_graph, scene_graph_context, adjust_X_G=False, textbox=None, meshcat=None):\n",
    "    body = plant.GetBodyByName(\"body\")\n",
    "    X_G = plant.GetFreeBodyPose(plant_context, body)\n",
    "\n",
    "    # Transform cloud into gripper frame\n",
    "    X_GW = X_G.inverse()\n",
    "    pts = np.asarray(cloud.points).T\n",
    "    p_GC = X_GW.multiply(pts)\n",
    "\n",
    "    # Crop to a region inside of the finger box.\n",
    "    crop_min = [-.05, 0.1, -0.00625]\n",
    "    crop_max = [.05, 0.1125, 0.00625]\n",
    "    indices = np.all((crop_min[0] <= p_GC[0,:], p_GC[0,:] <= crop_max[0],\n",
    "                      crop_min[1] <= p_GC[1,:], p_GC[1,:] <= crop_max[1],\n",
    "                      crop_min[2] <= p_GC[2,:], p_GC[2,:] <= crop_max[2]), \n",
    "                     axis=0)\n",
    "\n",
    "    if meshcat:\n",
    "        draw_points(meshcat[\"points\"], pts[:, indices], [1., 0, 0], size=0.01)\n",
    "\n",
    "    if adjust_X_G and np.sum(indices)>0:\n",
    "        p_GC_x = p_GC[0, indices]\n",
    "        p_Gcenter_x = (p_GC_x.min() + p_GC_x.max())/2.0\n",
    "        X_G.set_translation(X_G.translation() + X_G.rotation().multiply([p_Gcenter_x, 0, 0]))\n",
    "        plant.SetFreeBodyPose(plant_context, body, X_G)\n",
    "        X_GW = X_G.inverse()\n",
    "\n",
    "    query_object = scene_graph.get_query_output_port().Eval(scene_graph_context)\n",
    "    # Check collisions between the gripper and the sink\n",
    "    if query_object.HasCollisions():\n",
    "        cost = np.inf\n",
    "        if textbox:\n",
    "            textbox.value = \"Gripper is colliding with the sink!\\n\"\n",
    "            textbox.value += f\"cost: {cost}\"\n",
    "        return cost\n",
    "\n",
    "    # Check collisions between the gripper and the point cloud\n",
    "    margin = 0.0  # must be smaller than the margin used in the point cloud preprocessing.\n",
    "    for pt in cloud.points:\n",
    "        distances = query_object.ComputeSignedDistanceToPoint(pt, threshold=margin)\n",
    "        if distances:\n",
    "            cost = np.inf\n",
    "            if textbox:\n",
    "                textbox.value = \"Gripper is colliding with the point cloud!\\n\"\n",
    "                textbox.value += f\"cost: {cost}\"\n",
    "            return cost\n",
    "\n",
    "\n",
    "    n_GC = X_GW.rotation().multiply(np.asarray(cloud.normals)[indices,:].T)\n",
    "\n",
    "    # Penalize deviation of the gripper from vertical.\n",
    "    # weight * -dot([0, 0, -1], R_G * [0, 1, 0]) = weight * R_G[2,1]\n",
    "    cost = 20.0*X_G.rotation().matrix()[2, 1]\n",
    "\n",
    "    # Reward sum |dot product of normals with gripper x|^2\n",
    "    cost -= np.sum(n_GC[0,:]**2)\n",
    "\n",
    "    if textbox:\n",
    "        textbox.value = f\"cost: {cost}\\n\"\n",
    "        textbox.value += \"normal terms:\" + str(n_GC[0,:]**2)\n",
    "    return cost\n",
    "\n",
    "\n",
    "def process_point_cloud(diagram, context, cameras, bin_name):\n",
    "    \"\"\"A \"no frills\" version of the example above, that returns the down-sampled point cloud\"\"\"\n",
    "    plant = diagram.GetSubsystemByName(\"plant\")\n",
    "    plant_context = plant.GetMyContextFromRoot(context)\n",
    "\n",
    "    # Compute crop box.\n",
    "    bin_instance = plant.GetModelInstanceByName(bin_name)\n",
    "    bin_body = plant.GetBodyByName(\"bin_base\", bin_instance)\n",
    "    X_B = plant.EvalBodyPoseInWorld(plant_context, bin_body)\n",
    "    margin = 0.001  # only because simulation is perfect!\n",
    "    a = X_B.multiply([-.22+0.025+margin, -.29+0.025+margin, 0.015+margin])\n",
    "    b = X_B.multiply([.22-0.1-margin, .29-0.025-margin, 2.0])\n",
    "    crop_min = np.minimum(a,b)\n",
    "    crop_max = np.maximum(a,b)\n",
    "\n",
    "    # Evaluate the camera output ports to get the images.\n",
    "    merged_pcd = o3d.geometry.PointCloud()\n",
    "    for c in cameras:\n",
    "        point_cloud = diagram.GetOutputPort(f\"{c}_point_cloud\").Eval(context)\n",
    "        pcd = create_open3d_point_cloud(point_cloud)\n",
    "\n",
    "        # Crop to region of interest.\n",
    "        pcd = pcd.crop(\n",
    "            o3d.geometry.AxisAlignedBoundingBox(min_bound=crop_min,\n",
    "                                                max_bound=crop_max))    \n",
    "\n",
    "        pcd.estimate_normals(search_param=o3d.geometry.KDTreeSearchParamHybrid(\n",
    "            radius=0.1, max_nn=30))\n",
    "\n",
    "        camera = plant.GetModelInstanceByName(c)\n",
    "        body = plant.GetBodyByName(\"base\", camera)\n",
    "        X_C = plant.EvalBodyPoseInWorld(plant_context, body)\n",
    "        pcd.orient_normals_towards_camera_location(X_C.translation())\n",
    "        \n",
    "        # Merge point clouds.\n",
    "        merged_pcd += pcd\n",
    "\n",
    "    # Voxelize down-sample.  (Note that the normals still look reasonable)\n",
    "    return merged_pcd.voxel_down_sample(voxel_size=0.005)\n",
    "\n",
    "def make_environment_model(directive=None, draw=False, rng=None, num_ycb_objects=0, bin_name=\"bin0\"):\n",
    "    # Make one model of the environment, but the robot only gets to see the sensor outputs.\n",
    "    if not directive:\n",
    "        directive = FindResource(\"models/two_bins_w_cameras.yaml\")\n",
    "\n",
    "    builder = DiagramBuilder()\n",
    "    plant, scene_graph = AddMultibodyPlantSceneGraph(builder, time_step=0.0005)\n",
    "    parser = Parser(plant)\n",
    "    parser.package_map().PopulateFromFolder(FindResource(\"\"))\n",
    "    ProcessModelDirectives(LoadModelDirectives(directive), plant, parser)\n",
    "\n",
    "    for i in range(num_ycb_objects):\n",
    "        object_num = rng.integers(len(ycb))\n",
    "        sdf = FindResourceOrThrow(\"drake/manipulation/models/ycb/sdf/\" + ycb[object_num][1])\n",
    "        parser.AddModelFromFile(sdf, f\"object{i}\")\n",
    "\n",
    "    plant.Finalize()\n",
    "    AddRgbdSensors(builder, plant, scene_graph)\n",
    "\n",
    "    if draw:\n",
    "        meshcat = ConnectMeshcatVisualizer(builder, scene_graph, zmq_url=zmq_url, prefix=\"environment\")\n",
    "\n",
    "    diagram = builder.Build()\n",
    "    context = diagram.CreateDefaultContext()\n",
    "\n",
    "    if num_ycb_objects > 0:\n",
    "        generator = RandomGenerator(rng.integers(1000))  # this is for c++\n",
    "        plant_context = plant.GetMyContextFromRoot(context)\n",
    "        bin_instance = plant.GetModelInstanceByName(bin_name)\n",
    "        bin_body = plant.GetBodyByName(\"bin_base\", bin_instance)\n",
    "        X_B = plant.EvalBodyPoseInWorld(plant_context, bin_body)\n",
    "        z = 0.1\n",
    "        for body_index in plant.GetFloatingBaseBodies():\n",
    "            tf = RigidTransform(\n",
    "                    UniformlyRandomRotationMatrix(generator),  \n",
    "                    [rng.uniform(-.15,.15), rng.uniform(-.2, .2), z])\n",
    "            plant.SetFreeBodyPose(plant_context, \n",
    "                                plant.get_body(body_index),\n",
    "                                X_B.multiply(tf))\n",
    "            z += 0.1\n",
    "\n",
    "        simulator = Simulator(diagram, context)\n",
    "        simulator.AdvanceTo(1.0 if running_as_notebook else 0.1)\n",
    "    elif draw:\n",
    "        meshcat.load()\n",
    "        diagram.Publish(context)\n",
    "\n",
    "\n",
    "    return diagram, context\n",
    "\n",
    "def grasp_score_inspector():\n",
    "    v = meshcat.Visualizer(zmq_url=zmq_url)\n",
    "    v.delete()\n",
    "    environment, environment_context = make_environment_model(directive = FindResource(\"models/clutter_mustard.yaml\"))\n",
    "\n",
    "    # Another diagram for the objects the robot \"knows about\": gripper, cameras, bins.  Think of this as the model in the robot's head.\n",
    "    builder = DiagramBuilder()\n",
    "    plant, scene_graph = AddMultibodyPlantSceneGraph(builder, time_step=0.001)\n",
    "    parser = Parser(plant)\n",
    "    parser.package_map().PopulateFromFolder(FindResource(\"\"))\n",
    "    ProcessModelDirectives(LoadModelDirectives(FindResource(\"models/clutter_planning.yaml\")), plant, parser)\n",
    "    plant.Finalize()\n",
    "    \n",
    "    v = ConnectMeshcatVisualizer(builder, scene_graph, zmq_url=zmq_url, prefix=\"planning\")\n",
    "    v.load()\n",
    "    diagram = builder.Build()\n",
    "    context = diagram.CreateDefaultContext()\n",
    "\n",
    "    cloud = process_point_cloud(environment, environment_context, [\"camera0\", \"camera1\", \"camera2\"], \"bin0\")\n",
    "    draw_open3d_point_cloud(v.vis[\"cloud\"], cloud, size=0.003)\n",
    "\n",
    "    textbox = Textarea(description=\"cost info\", layout={'width': '800px', 'height': '100px'})\n",
    "    display(textbox)\n",
    "\n",
    "    lower_limit = [-np.pi, -np.pi/4., -np.pi/4., -1, -1, 0]\n",
    "    upper_limit = [0, np.pi/4., np.pi/4., 1, 1, 1]\n",
    "    sliders = MakeJointSlidersThatPublishOnCallback(\n",
    "        plant, diagram, context, my_callback=partial(\n",
    "            grasp_candidate_cost, cloud=cloud, plant=plant, scene_graph=scene_graph,\n",
    "            scene_graph_context=scene_graph.GetMyContextFromRoot(context),\n",
    "            textbox=textbox, meshcat=v.vis[\"cost\"]),\n",
    "        lower_limit=lower_limit, upper_limit=upper_limit)\n",
    "    sliders[0].value = -np.pi/2.0\n",
    "    sliders[3].value = -0.05\n",
    "    sliders[4].value = -.5\n",
    "    sliders[5].value = 0.25\n",
    "\n",
    "grasp_score_inspector()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "colab_type": "text",
    "id": "WFE6URSynIT3"
   },
   "source": [
    "# Inverse kinematics as an optimization"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "colab": {},
    "colab_type": "code",
    "id": "QYdDyygnnGaO"
   },
   "outputs": [],
   "source": [
    "def teleop_inverse_kinematics(grasp_cylinder=True):\n",
    "    builder = pydrake.systems.framework.DiagramBuilder()\n",
    "\n",
    "    plant, scene_graph = pydrake.multibody.plant.AddMultibodyPlantSceneGraph(builder, time_step=0.001)\n",
    "    iiwa = AddIiwa(plant, \"with_box_collision\")\n",
    "    wsg = AddWsg(plant, iiwa, welded=True)\n",
    "    cylinder = AddShape(plant, pydrake.geometry.Cylinder(0.02, 1.0), \"cylinder\")\n",
    "    plant.Finalize()\n",
    "\n",
    "    visualizer = pydrake.systems.meshcat_visualizer.ConnectMeshcatVisualizer(\n",
    "        builder, \n",
    "        scene_graph, \n",
    "        zmq_url=zmq_url,\n",
    "        server_args=server_args,\n",
    "        delete_prefix_on_load=False)\n",
    "\n",
    "    diagram = builder.Build()\n",
    "    context = diagram.CreateDefaultContext()\n",
    "    plant_context = plant.GetMyContextFromRoot(context)\n",
    "\n",
    "    q0 = plant.GetPositions(plant_context)\n",
    "    gripper_frame = plant.GetFrameByName(\"body\", wsg)\n",
    "    cylinder_body = plant.GetBodyByName(\"cylinder\", cylinder)\n",
    "    cylinder_frame = plant.GetFrameByName(\"cylinder\", cylinder)\n",
    "\n",
    "    console = Textarea(value=\"\", description=\"\", layout={'width':'60%','height':'100px'}, style={'description_width':'initial'})\n",
    "\n",
    "    def my_callback(context, pose):\n",
    "        ik = pydrake.multibody.inverse_kinematics.InverseKinematics(plant, plant_context)\n",
    "        ik.AddPositionConstraint(cylinder_frame, [0, 0, 0], plant.world_frame(), pose.translation(), pose.translation())\n",
    "        ik.AddOrientationConstraint(cylinder_frame, RotationMatrix(), plant.world_frame(), pose.rotation(), 0.0)\n",
    "        if grasp_cylinder:\n",
    "            ik.AddPositionConstraint(gripper_frame, [0, 0.1, -0.02], cylinder_frame, [0, 0, -0.5], [0, 0, 0.5])\n",
    "            ik.AddPositionConstraint(gripper_frame, [0, 0.1, 0.02], cylinder_frame, [0, 0, -0.5], [0, 0, 0.5])\n",
    "        else:\n",
    "            ik.AddMinimumDistanceConstraint(0.001, 0.1)\n",
    "        prog = ik.get_mutable_prog()\n",
    "        q = ik.q()\n",
    "        prog.AddQuadraticErrorCost(np.identity(len(q)), q0, q)\n",
    "        prog.SetInitialGuess(q, q0)\n",
    "        result = pydrake.solvers.mathematicalprogram.Solve(ik.prog())\n",
    "        if not result.is_success():\n",
    "            console.value += \"IK failed\\n\"\n",
    "\n",
    "    visualizer.load()\n",
    "    X_WC = RigidTransform(RollPitchYaw(np.pi/2.0, 0, 0), [0.5, 0, 0.5])\n",
    "    s = MakePoseSlidersThatPublishOnCallback(visualizer, context, my_callback, value=X_WC)\n",
    "    display(console)\n",
    "\n",
    "# Set grasp_cylinder=False if you just want to antagonize the robot with a stick.\n",
    "teleop_inverse_kinematics(grasp_cylinder=True)\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "colab": {},
    "colab_type": "code",
    "id": "Z1451ebPrMwu"
   },
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "colab": {
   "collapsed_sections": [],
   "name": "Robotic Manipulation - A few hightlights.ipynb",
   "provenance": [],
   "toc_visible": true
  },
  "kernelspec": {
   "display_name": "Python 3",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.6.9"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 1
}